from Utils.util import *
import sys

def plan(OG_Map, Map):
    n = len(OG_Map)
    temp_map = OG_Map.copy()
    start_x, start_y = [4, 2]
    end_x, end_y = [n - 6, n - 4]
    path = [[start_x, start_y]]
    sys.setrecursionlimit(100000)
    dfs(start_x, start_y, temp_map, end_x, end_y, path)
    # print(path)
    draw_path(path, OG_Map, Map)

def dfs(start_x, start_y, OG_Map, end_x, end_y, path):
    """
    深度优先搜索算法路径规划
    """
    for i in range(4):
        dir = [[1, 0], [0, 1], [-1, 0], [0, -1]]
        nx, ny = start_x + dir[i][0], start_y + dir[i][1]
        if nx < 0 or ny < 0 or nx > len(OG_Map) - 1 or ny > len(OG_Map) - 1:
            continue
        if nx == end_x and ny == end_y:
            path.append([nx, ny])
            return True
        if OG_Map[nx, ny] == 1:
            OG_Map[nx, ny] = 0
            path.append([nx, ny])
            flag = dfs(nx, ny, OG_Map, end_x, end_y, path)
            if flag:
                return True
            OG_Map[nx, ny] = 1
    path.pop()
    return False